#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_
#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_

#include "cartographer/common/time.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/pose_extrapolator_interface.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer/rcm/imu/initialize.h"
#include "cartographer/rcm/imu/pose_integrate.h"

namespace cartographer
{
    namespace mapping
    {
        inline bool IsDurationExceed(const common::Time &time1, const common::Time &time2, const common::Duration &duration)
        {
            if (time1 > time2)
            {
                const double delta = common::ToSeconds(time1 - time2);
                if (delta > common::ToSeconds(duration))
                    return true;
            }
            else if (time1 < time2)
            {
                const double delta = common::ToSeconds(time2 - time1);
                if (delta > common::ToSeconds(duration))
                    return true;
            }
            return false;
        }

        /*
        主要用于点云去畸变和为点云配准提供初始位姿;位姿计算并不是使用积分思路，而是用局部建图得到的位姿作插值来得到新的位姿;
        也就是说该类的位姿依赖于局部建图结果;而插值需要线速和角速，线速是通过两个pose或者odom除以时间得到的;角速没什么用，位姿的
        姿态角由ImuTracker来维护;即ImuTracker负责旋转量，该类负责平移量;
        */
        class PoseExtrapolator : public PoseExtrapolatorInterface
        {
        public:
            explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant);

            PoseExtrapolator(const PoseExtrapolator &) = delete;

            PoseExtrapolator &operator=(const PoseExtrapolator &) = delete;

            /// @brief 该函数没有被用到
            /// @param pose_queue_duration
            /// @param imu_gravity_time_constant
            /// @param imu_data
            /// @return
            static std::unique_ptr<PoseExtrapolator> InitializeWithImu(
                const common::Duration pose_queue_duration,
                const double imu_gravity_time_constant,
                const sensor::ImuData &imu_data);

            // Returns the time of the last added pose or Time::min() if no pose was added yet.
            common::Time GetLastPoseTime() const;

            Eigen::Vector3d GetLinearVelocity();

            common::Time GetLastExtrapolatedTime() const;

            void PrintPose(const char *prefix, const TimedPose &pose);

            void AddImuData(const sensor::ImuData &imu_data) override;

            void AddPose(common::Time time, const transform::Rigid3d &pose) override;

            void AddOdometryData(const sensor::OdometryData &odometry_data) override;

            transform::Rigid3d ExtrapolatePose(common::Time time) override;

            transform::Rigid3d ExtrapolateRobotPose(const common::Time time);

            ExtrapolationResult ExtrapolatePosesWithGravity(const std::vector<common::Time> &times) override;

            // Returns the current gravity alignment estimate as a rotation from
            // the tracking frame into a gravity aligned frame.
            Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

            bool use_imu_integrate = false;

        private:
            void TrimImuData();

            void TrimOdometryData();

            void UpdateVelocitiesFromPoses();

            void AdvanceImuTracker(common::Time time, ImuTracker *imu_tracker) const;

            Eigen::Vector3d ExtrapolateTranslation(common::Time time);

            Eigen::Quaterniond ExtrapolateRotation(common::Time time, ImuTracker *imu_tracker) const;

            const double gravity_time_constant_;
            const common::Duration pose_queue_duration_;

            // PoseIntegrator::Ptr poseIntegrator_;
            TimedPose cached_extrapolated_pose_;
            std::deque<sensor::ImuData> imu_data_;
            // 存储局部建图得到的位姿
            std::deque<TimedPose> timed_pose_queue_;
            std::deque<sensor::OdometryData> odometry_data_;

            // 因为传感器和调用频率不同，所以需要三个ImuTracker，使其互不影响;以局部建图(点云消息)频率为主频，所以每次局部
            // 建图结束后,用imu_tracker_重置其他两个ImuTracker,因为该类的位姿基准是局部建图位姿;注意姿态角也是以imu_tracker_
            // 里的姿态来求增量的

            // 专门用来跟踪机器人当前姿态，局部建图一次更新一次;与点云消息同频;
            std::unique_ptr<ImuTracker> imu_tracker_;
            // 专门用来跟踪里程计的姿态，因为里程计不在tracking系，要用这个姿态把里程计变换到tracking系,然后才能算线速;
            // 与里程计消息同频
            std::unique_ptr<ImuTracker> odometry_imu_tracker_;
            // 专门用来给外部模块提供插值用的;在点云消畸变时,处理每个点时都会调用一次;最频繁
            std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
            // 通过局部建图位姿平均得到的线速
            Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
            // 通过两个odom数据平均得到的线速
            Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();

            Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();
            Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
        };
    } // namespace mapping
} // namespace cartographer

#endif
